from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction
from launch.substitutions import (Command, FindExecutable, LaunchConfiguration,
                                  PathJoinSubstitution)


def launch_setup(context, *args, **kwargs):

    namespace = LaunchConfiguration("namespace")

    # General arguments
    description_package = LaunchConfiguration("description_package")
    description_file = LaunchConfiguration("description_file")

    control_package = LaunchConfiguration("control_package")
    control_file = LaunchConfiguration("control_file")

    # Get URDF via xacro
    robot_description_content = Command(
        [
            PathJoinSubstitution([FindExecutable(name="xacro")]),
            " ",
            PathJoinSubstitution([FindPackageShare(description_package), "urdf", description_file]),
            " ",
            "namespace:=",
            namespace,
            " ",
        ]
    )
    robot_description = {"robot_description": robot_description_content}

    config_oberon7_controller = PathJoinSubstitution(
        [FindPackageShare(control_package),
        "config",
        control_file],
    )

    controller_manager_node = Node(
        package="controller_manager",
        executable="ros2_control_node",
        parameters=[robot_description, config_oberon7_controller],
        output={
            "stdout": "screen",
            "stderr": "screen",
        },
    )

    joint_traj_controller_node = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["joint_trajectory_controller"],
        output="screen",
    )

    nodes_to_start = [
        controller_manager_node, 
        joint_traj_controller_node,
        ]

    return nodes_to_start

def generate_launch_description():

    declared_arguments = []

    declared_arguments.append(
        DeclareLaunchArgument(
            "namespace",
            default_value="oberon7",
            description="Specify the namespace of the URDF file.",
        )
    )
    # General arguments
    declared_arguments.append(
        DeclareLaunchArgument(
            "description_package",
            default_value="oberon7_description",
            description="Description package with robot URDF/XACRO files. Usually the argument \
        is not set, it enables use of a custom description.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "description_file",
            default_value="oberon7_robot.urdf.xacro",
            description="URDF/XACRO description file with the robot.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "control_package",
            default_value="oberon7_robot_driver",
            description="Oberon7 robot driver package.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "control_file",
            default_value="controllers.yaml",
            description="Oberon7 ros2 controllers config file.",
        )
    )

    return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])


